//
// Created by ashkore on 23-9-2.
//

#include "../../Inc/HC_SR04/sr04.h"
#include <stdlib.h>
#define DISTANCE_LIMIT 5000

void sr04_init(sr04_t *sr04_struct){
  // Enable trigger pin
  HAL_GPIO_WritePin(sr04_struct->trig_port, sr04_struct->trig_pin, GPIO_PIN_RESET);
  // Set input capture edge to rising
  __HAL_TIM_SET_CAPTUREPOLARITY(sr04_struct->echo_htim, sr04_struct->echo_channel, TIM_INPUTCHANNELPOLARITY_RISING);
  // Set capture flag to 0
  sr04_struct->capture_flag = 0;
  // Enable echo pin
  HAL_TIM_IC_Start_IT(sr04_struct->echo_htim, sr04_struct->echo_channel);
  HAL_TIM_Base_Start_IT(sr04_struct->echo_htim);
}

void sr04_trigger(sr04_t *sr04_struct){
  // Send 10us pulse to trigger pin
  HAL_GPIO_WritePin(sr04_struct->trig_port, sr04_struct->trig_pin, GPIO_PIN_SET);
  
  // 简单的循环延时（大约10μs）
  // 72MHz时钟，每个循环约1μs
  for(volatile int i = 0; i < 720; i++); // 约10μs
  
  HAL_GPIO_WritePin(sr04_struct->trig_port, sr04_struct->trig_pin, GPIO_PIN_RESET);
}

void sr04_read_distance(sr04_t *sr04_struct) {
  switch (sr04_struct->capture_flag){
    case 0:
      // 上升沿：记录开始时间
      sr04_struct->start_counter = __HAL_TIM_GET_COUNTER(sr04_struct->echo_htim);
      sr04_struct->capture_flag = 1;
      sr04_struct->tim_update_count = 0;
      __HAL_TIM_SET_CAPTUREPOLARITY(sr04_struct->echo_htim, sr04_struct->echo_channel, TIM_INPUTCHANNELPOLARITY_FALLING);
      break;
      
    case 1:
      // 下降沿：计算距离
      sr04_struct->end_counter = __HAL_TIM_GET_COUNTER(sr04_struct->echo_htim);
      sr04_struct->capture_flag = 0;
      
      // 计算时间差（微秒）
      uint32_t time_us;
      if (sr04_struct->end_counter >= sr04_struct->start_counter) {
        // 正常情况
        time_us = sr04_struct->end_counter - sr04_struct->start_counter;
      } else {
        // 定时器溢出
        time_us = (65536 - sr04_struct->start_counter) + sr04_struct->end_counter;
      }
      
      // 加上溢出次数
      time_us += sr04_struct->tim_update_count * 65536;
      
      // 距离计算（毫米）
      // 声速 = 340 m/s = 0.34 mm/μs
      // 往返距离 = time_us * 0.34 / 2 = time_us * 0.17
      // 为避免浮点运算：distance_mm = time_us * 17 / 100
      uint32_t distance_mm = (time_us * 17) / 100;
      
      // 有效性检查
      if (time_us < 58 || time_us > 23200) {
        // 小于1cm(58μs)或大于4m(23200μs)认为无效
        // 保持上次值
      } else {
        // 简单滤波：变化超过5mm才更新
        int diff = (int)distance_mm - (int)sr04_struct->last_distance;
        if (abs(diff) > 5 || sr04_struct->last_distance == 0) {
          sr04_struct->distance = distance_mm;
          sr04_struct->last_distance = distance_mm;
        }
      }
      
      __HAL_TIM_SET_CAPTUREPOLARITY(sr04_struct->echo_htim, sr04_struct->echo_channel, TIM_INPUTCHANNELPOLARITY_RISING);
      break;
  }
}
